#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <std_msgs/msg/empty.hpp>
#include <serial/serial.h>
#include <customized_msgs/msg/feed_back.hpp>
#include <queue>
#include <geometry_msgs/msg/point.hpp>
#include <std_msgs/msg/empty.hpp>
#include <chrono>
#include <customized_msgs/msg/gcs_cmd.hpp>
// #include <customized_msgs/msg/drone_state_gcs.hpp>
#include <customized_msgs/msg/drone_whole_state.hpp>
#include <customized_msgs/msg/topic_frequencies.hpp>
#include <customized_msgs/msg/winch_state.hpp>
#include "custom_filters/MeanFilter.hpp"
#include <customized_msgs/msg/ground_detection.hpp>
#include <sensor_msgs/msg/range.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <customized_msgs/msg/arg_set_length.hpp>
#include "can_motor_driver/crc_checker.hpp"

// #define SERIAL_DEBUG true
#define SERIAL_DEBUG false

#define GCS_ENABLE true

#define ANSI_COLOR_RED "\x1b[31m"
#define ANSI_COLOR_GREEN "\x1b[32m"
#define ANSI_COLOR_YELLOW "\x1b[33m"
#define ANSI_COLOR_RESET "\x1b[0m"

class WinchWirelessServer : public rclcpp::Node
{
public:
    WinchWirelessServer() : Node("winch_wireless_server"), ser(), crc_checker()
    {
        this->declare_parameter<std::string>("serial_port_name", "/dev/ttyUSB1");
        this->declare_parameter<double>("control_frequency", 100.0);

        this->get_parameter("serial_port_name", serial_port_name);
        this->get_parameter("control_frequency", control_frequency);

        RCLCPP_WARN(this->get_logger(), "starting winch_wireless_server at %f hz  @%s port", control_frequency, serial_port_name.c_str());

        timer = this->create_wall_timer(
            std::chrono::duration<double>(1.0 / control_frequency),
            std::bind(&WinchWirelessServer::timerCallback, this));

        motor_fb_sub = this->create_subscription<customized_msgs::msg::FeedBack>(
            "/motor_feedback/motor1_winch", 10, std::bind(&WinchWirelessServer::motor_fb_callback, this, std::placeholders::_1));

        gcs_cmd_pub = this->create_publisher<customized_msgs::msg::GcsCmd>("/winch_wireless_server/gcs_cmd", 1000);
        odom_pub = this->create_publisher<nav_msgs::msg::Odometry>("/winch_wireless_server/grabber_odom", 1000);
        winch_length_pub = this->create_publisher<customized_msgs::msg::ArgSetLength>("/winch_wireless_server/set_length", 1000);
        winch_state_sub = this->create_subscription<customized_msgs::msg::WinchState>(
            "/winch_ctrl_winch/winch_state", 10, std::bind(&WinchWirelessServer::winch_state_callback, this, std::placeholders::_1));
        ground_height_pub = this->create_publisher<customized_msgs::msg::GroundDetection>("/ground_detector/ground_height_winch", 1000);

        motor_fb_msg.feedback_angle = uninitialized_double;
        motor_fb_msg.feedback_velocity = uninitialized_double;
        motor_fb_msg.feedback_torque = uninitialized_double;
        motor_fb_msg.mos_temperature = uninitialized_int;
        motor_fb_msg.rotor_temperature = uninitialized_int;
        winch_state_msg.target_length = uninitialized_double;

        last_recv_grabber_cmd_time = this->now();
        if (!ser.isOpen())
        {
            try
            {
                OpenSerial();
            }
            catch (const serial::IOException &e)
            {
                RCLCPP_ERROR(this->get_logger(), "Unable to open port ");
            }
        }

        if (ser.isOpen())
        {
            try
            {
                update_send_data();
            }
            catch (const serial::SerialException &e)
            {
                RCLCPP_ERROR(this->get_logger(), "Serial error");
            }
        }
    }

    void winch_state_callback(const customized_msgs::msg::WinchState::SharedPtr msg)
    {
        winch_state_msg = *msg;
    }

    void motor_fb_callback(const customized_msgs::msg::FeedBack::SharedPtr msg)
    {
        motor_fb_msg = *msg;
    }

    void update_send_data()
    {
        winch_send_data[1] = crc_checker.generate_random_number();

        uint8_t encode_buffer_2bytes[2];
        uint8_t encode_buffer_4bytes[4];

        double elapsed_time = (this->now() - motor_fb_msg.stamp).seconds();
        double time_out = 1.0;
        if (elapsed_time > time_out)
        {
            motor_fb_msg.feedback_angle = uninitialized_double;
            motor_fb_msg.feedback_velocity = uninitialized_double;
            motor_fb_msg.feedback_torque = uninitialized_double;
            motor_fb_msg.mos_temperature = uninitialized_int;
            motor_fb_msg.rotor_temperature = uninitialized_int;
        }
        elapsed_time = (this->now() - winch_state_msg.stamp).seconds();

        if (elapsed_time > time_out)
        {
            winch_state_msg.target_length = uninitialized_double;
        }

        int index = 2;
        encodeFloatTo2Bytes(motor_fb_msg.feedback_angle, encode_buffer_2bytes);
        winch_send_data[index++] = encode_buffer_2bytes[0];
        winch_send_data[index++] = encode_buffer_2bytes[1];

        encodeFloatTo2Bytes(motor_fb_msg.feedback_velocity, encode_buffer_2bytes);
        winch_send_data[index++] = encode_buffer_2bytes[0];
        winch_send_data[index++] = encode_buffer_2bytes[1];

        encodeFloatTo2Bytes(motor_fb_msg.feedback_torque, encode_buffer_2bytes);
        winch_send_data[index++] = encode_buffer_2bytes[0];
        winch_send_data[index++] = encode_buffer_2bytes[1];

        encodeFloatTo2Bytes(winch_state_msg.target_length, encode_buffer_2bytes);
        winch_send_data[index++] = encode_buffer_2bytes[0];
        winch_send_data[index++] = encode_buffer_2bytes[1];

        winch_send_data[index++] = motor_fb_msg.mos_temperature;
        winch_send_data[index++] = motor_fb_msg.rotor_temperature;

        std::vector<unsigned char> winch_send_data_vec(winch_send_data, winch_send_data + winch_send_data_length);

        uint32_t crc = crc_checker.calculate_crc(winch_send_data_vec, winch_send_data_vec.size() - 5);
        uint8_t crc_1 = ((crc >> 24) & 0xFF);
        uint8_t crc_2 = ((crc >> 16) & 0xFF);
        uint8_t crc_3 = ((crc >> 8) & 0xFF);
        uint8_t crc_4 = (crc & 0xFF);
        winch_send_data[winch_send_data_length - 5] = crc_1;
        winch_send_data[winch_send_data_length - 4] = crc_2;
        winch_send_data[winch_send_data_length - 3] = crc_3;
        winch_send_data[winch_send_data_length - 2] = crc_4;

        if (SERIAL_DEBUG)
        {
            RCLCPP_INFO(this->get_logger(), "winch_send_data:");
            printHex(winch_send_data, winch_send_data_length);
        }

        ser.write(winch_send_data, winch_send_data_length);
    }

    void encodeFloatTo2Bytes(float value, uint8_t *buffer)
    {
        int16_t int_value = static_cast<int16_t>(value * 100);
        std::memcpy(buffer, &int_value, sizeof(int16_t));
    }

    void encodeFloatTo4Bytes(float value, uint8_t *buffer)
    {
        int32_t int_value = static_cast<int32_t>(value * 1000000);
        std::memcpy(buffer, &int_value, sizeof(int32_t));
    }

    void OpenSerial()
    {
        ser.setPort(serial_port_name);

        RCLCPP_INFO(this->get_logger(), "openning port: %s", serial_port_name.c_str());

        ser.setBaudrate(115200);
        serial::Timeout to = serial::Timeout::simpleTimeout(1000);
        ser.setTimeout(to);
        ser.open();
        ClearSerialBuffer();
    }

    void printHex(const uint8_t *data, std::size_t size)
    {
        std::stringstream ss;
        ss << std::hex << std::setw(2) << std::setfill('0');
        for (std::size_t i = 0; i < size; ++i)
        {
            ss << std::setw(2) << static_cast<int>(data[i]) << ' ';
        }
        RCLCPP_INFO(this->get_logger(), ss.str().c_str());
    }

    void printHex(const std::vector<uint8_t> &data)
    {
        std::stringstream ss;
        ss << std::hex << std::setw(2) << std::setfill('0');
        for (std::size_t i = 0; i < data.size(); ++i)
        {
            ss << std::setw(2) << static_cast<int>(data[i]) << ' ';
        }
        RCLCPP_INFO(this->get_logger(), ss.str().c_str());
    }
    std::vector<uint8_t> queue2vector(std::queue<uint8_t> queue)
    {
        std::vector<uint8_t> vec;
        while (!queue.empty())
        {
            vec.push_back(queue.front());
            queue.pop();
        }

        return vec;
    }

    std::vector<uint8_t> queue2vector(std::queue<uint8_t> queue, int begin, int end)
    {
        int count = 0;
        std::vector<uint8_t> vec;
        while (!queue.empty())
        {
            if (count >= begin && count < end)
                vec.push_back(queue.front());
            count++;
            queue.pop();
        }
        return vec;
    }

    void timerCallback()
    {
        read_ttl_data_by_bite();
        return;
    }

    void clear_ttl_data_queue(int begin, int end)
    {
        std::deque<int> temp_deque;

        while (!recv_can_data_queue.empty())
        {
            temp_deque.push_back(recv_can_data_queue.front());
            recv_can_data_queue.pop();
        }

        // RCLCPP_INFO(this->get_logger(), "temp_deque size = %d begin = %d end = %d", temp_deque.size(), begin, end);

        if (begin >= 0 && end < temp_deque.size() && begin <= end)
        {
            temp_deque.erase(temp_deque.begin() + begin, temp_deque.begin() + end + 1);
        }

        for (int elem : temp_deque)
        {
            recv_can_data_queue.push(elem);
        }
        // RCLCPP_INFO(this->get_logger(), "temp_deque size = %d after clear queue:", temp_deque.size());
        // printHex(queue2vector(recv_can_data_queue));
    }

    void ClearSerialBuffer()
    {
        RCLCPP_WARN(this->get_logger(), "data is blocked!! clear buffer");
        if (!ser.isOpen())
        {
            std::cerr << "Serial port is not open." << std::endl;
            return;
        }
        // int count = 0;
        // Clear input buffer
        // while (ser.available() > 0) {
        //     std::vector<uint8_t> buffer(ser.available());
        //     ser.read(buffer, buffer.size());
        //     count += buffer.size();
        // }

        std::vector<uint8_t> buffer(ser.available());
        ser.read(buffer, buffer.size());

        RCLCPP_WARN(this->get_logger(), "data is blocked!! clear buffer:");
        printHex(buffer);

        // You might also want to write a dummy byte and then flush the output buffer
        ser.flushOutput();
        // ROS_INFO("Serial buffer of %d bytes has been cleared.",count);
    }

    void read_ttl_data_by_bite()
    {
        rclcpp::Time current_time = this->now();

        // RCLCPP_INFO(this->get_logger(), "recv_can_data_queue size = %d", recv_can_data_queue.size());

        int max_read_length = 100;
        uint8_t recv_data[max_read_length];

        int data_len = ser.available();
        
        // RCLCPP_INFO(this->get_logger(), "data_len = %d", data_len);


        if (data_len <= 0)
            return;

        // RCLCPP_INFO(this->get_logger(), "data_len = %d", data_len);
        int recv_len = 0;
        if (data_len > 0 && data_len <= max_read_length)
            recv_len = ser.read(recv_data, data_len);
        else
        {
            ClearSerialBuffer();
            return;
        }

        // if (SERIAL_DEBUG && recv_len != grabber_pack_length)
        if (SERIAL_DEBUG && recv_len > 0)
        {
            RCLCPP_INFO(this->get_logger(), "recv_len = %d", recv_len);
            RCLCPP_WARN(this->get_logger(), "ttl data recv_data is ");
            printHex(recv_data, recv_len);
        }

        if (recv_len <= 0)
        {
            return;
        }

        rclcpp::Time end_time = this->now();
        // RCLCPP_INFO(this->get_logger(), "read %d bytes takes %f ms", recv_len, (end_time - current_time).seconds() * 1000.0);

        for (int i = 0; i < recv_len; i++)
        {
            recv_can_data_queue.push(recv_data[i]);
        }

        while (recv_can_data_queue.size() > (int)(1.5 * grabber_pack_length) )
            recv_can_data_queue.pop();

        if (recv_can_data_queue.size() >= grabber_pack_length)
            check_ttl_data();

        end_time = this->now();
        if (SERIAL_DEBUG)
        {
            RCLCPP_INFO(this->get_logger(), "one loop takes %f ms", (end_time - current_time).seconds() * 1000.0);
        }
    }
    double decode2BytesToFloat(uint8_t byte1, uint8_t byte2)
    {
        // 将两个字节转换为int16
        int16_t int_value = byte1 | (byte2 << 8);

        // 处理有符号整数的情况
        if (int_value & 0x8000)
        { // 如果最高位为1，则这是一个负数
            int_value = int_value - 0x10000;
        }

        return int_value / 100.0;
    }

    double decode4BytesToFloat(uint8_t byte1, uint8_t byte2, uint8_t byte3, uint8_t byte4)
    {
        // Combine the four bytes into a 32-bit integer
        int32_t int_value = byte1 | (byte2 << 8) | (byte3 << 16) | (byte4 << 24);

        // Handle the signed integer case
        if (int_value & 0x80000000)
        { // If the highest bit is 1, it is a negative number
            int_value = int_value - 0x100000000;
        }

        return int_value / 1000000.0;
    }
    void decode_grabber_data(std::vector<uint8_t> recv_data_vector)
    {

        double odom_x = decode2BytesToFloat(recv_data_vector[2], recv_data_vector[3]);
        double odom_y = decode2BytesToFloat(recv_data_vector[4], recv_data_vector[5]);
        double odom_z = decode2BytesToFloat(recv_data_vector[6], recv_data_vector[7]);

        if (odom_x != uninitialized_double || odom_y != uninitialized_double || odom_z != uninitialized_double)
        {
            nav_msgs::msg::Odometry odom_msg;
            odom_msg.header.stamp = this->now();
            odom_msg.header.frame_id = "odom";
            odom_msg.child_frame_id = "base_link";
            odom_msg.pose.pose.position.x = odom_x;
            odom_msg.pose.pose.position.y = odom_y;
            odom_msg.pose.pose.position.z = odom_z;

            odom_pub->publish(odom_msg);
        }
        else
        {
            // RCLCPP_INFO(this->get_logger(), "uninitialized odom data");
        }

        double winch_length_control = decode4BytesToFloat(recv_data_vector[8], recv_data_vector[9], recv_data_vector[10], recv_data_vector[11]);
        int winch_length_control_flag = recv_data_vector[12];

        if (fabs(winch_length_control) > 0.0000001 || winch_length_control_flag)
        {
            customized_msgs::msg::ArgSetLength set_length_msg;
            set_length_msg.length = winch_length_control;
            set_length_msg.abs_ctrl = winch_length_control_flag;
            set_length_msg.delay_time = 0;
            winch_length_pub->publish(set_length_msg);
        }

        int override_cmd = recv_data_vector[13];
        double manual_winch_speed = (double)(recv_data_vector[14] - 100) / 50.0;

        double ground_height = decode2BytesToFloat(recv_data_vector[15], recv_data_vector[16]);
        if (ground_height != uninitialized_double)
        {
            customized_msgs::msg::GroundDetection ground_height_msg;
            ground_height_msg.filtered_height = ground_height;
            ground_height_msg.stamp = this->now();
            ground_height_pub->publish(ground_height_msg);
        }
        else
        {
            // ROS_INFO("uninitialized ground height data");
        }

        customized_msgs::msg::GcsCmd gcs_cmd_msg;
        gcs_cmd_msg.cmd = override_cmd;
        gcs_cmd_msg.winch_speed = manual_winch_speed;
        gcs_cmd_msg.stamp = this->now();
        gcs_cmd_pub->publish(gcs_cmd_msg);

        last_recv_grabber_cmd_time = this->now();
        // ROS_INFO("target_angle = %f, target_velocity = %f, cmd = %d",target_angle,target_velocity,cmd);
    }

    bool check_grabber_data(std::vector<uint8_t> recv_data_vector)
    {
        // if(SERIAL_DEBUG)
        // {
        //     ROS_INFO("checking");
        //     printHex(recv_data_vector);
        // }
        // if(recv_data_vector[0] == 0xBB && recv_data_vector[1] == 0x01 && recv_data_vector[2] == 0x02 && recv_data_vector[3] == 0x03 && recv_data_vector[6] == 0xCC)
        if (recv_data_vector[0] == 0xDD && recv_data_vector[recv_data_vector.size() - 1] == 0xEE) // check header and tail
        {
            uint32_t crc = crc_checker.calculate_crc(recv_data_vector, recv_data_vector.size() - 5);
            if (crc_checker.check_crc(crc, recv_data_vector))
            {
                // ROS_INFO("check passed");
                decode_grabber_data(recv_data_vector);
            }
            return true; // clear the queue if the header and tail are correct
        }
        // ROS_INFO("check2");
        return false;
    }

    void check_ttl_data()
    {
        if (SERIAL_DEBUG)
        {
            RCLCPP_INFO(this->get_logger(), "checking");
            printHex(queue2vector(recv_can_data_queue));
        }
        for (int i = 0; i <= recv_can_data_queue.size() - grabber_pack_length; i++)
        {
            std::vector<uint8_t> recv_data_vector = queue2vector(recv_can_data_queue, i, i + grabber_pack_length);
            if (check_grabber_data(recv_data_vector))
            {
                if (SERIAL_DEBUG)
                {
                    RCLCPP_INFO(this->get_logger(), "before clearing recv data queue is ");
                    printHex(queue2vector(recv_can_data_queue));
                }
                clear_ttl_data_queue(i, i + grabber_pack_length - 1);
                // while(recv_can_data_queue.empty() == false)
                //     recv_can_data_queue.pop();

                update_send_data();
                if (SERIAL_DEBUG)
                {
                    RCLCPP_INFO(this->get_logger(), "left recv data queue is ");
                    printHex(queue2vector(recv_can_data_queue));
                }
                break;
            }
        }
    }

private:
    rclcpp::TimerBase::SharedPtr timer;

    std::string serial_port_name;
    double control_frequency;

    serial::Serial ser;

    rclcpp::Publisher<customized_msgs::msg::GcsCmd>::SharedPtr gcs_cmd_pub;
    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub;
    rclcpp::Publisher<customized_msgs::msg::ArgSetLength>::SharedPtr winch_length_pub;
    rclcpp::Publisher<customized_msgs::msg::GroundDetection>::SharedPtr ground_height_pub;
    rclcpp::Subscription<customized_msgs::msg::WinchState>::SharedPtr winch_state_sub;
    rclcpp::Subscription<customized_msgs::msg::FeedBack>::SharedPtr motor_fb_sub;

    customized_msgs::msg::FeedBack motor_fb_msg;
    customized_msgs::msg::WinchState winch_state_msg;

    double grabber_frequency = 5.0;

    int grabber_pack_length = 22;

    const double uninitialized_double = 0.12;
    const int uninitialized_int = 123;

    uint8_t winch_send_data[17] = {0xDD, 0x00, 0x01, 0x02, 0x01, 0x02, 0x01, 0x02, 0x01, 0x02, 0x03, 0x03, 0x01, 0x02, 0x03, 0x04, 0xEE};
    int winch_send_data_length = 17;

    std::queue<uint8_t> recv_can_data_queue;

    CrcChecker crc_checker;

    rclcpp::Time last_recv_grabber_cmd_time = this->now();
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<WinchWirelessServer>());
    rclcpp::shutdown();
    return 0;
}
